// SmartWAParam.cpp : Defines the entry point for the DLL application.
//

#include "stdafx.h"
#include "../GYWA/Common/SmartWAParam.h"
#include "assert.h"
#include "../GYWA/Common/CommonDef.h"

#define TIME_PER_POINT					0.06

#define OPEN_THRESHOLD_RANGE_LOW		0.681
#define OPEN_THRESHOLD_RANGE_HIGH		0.685

#define CLOSE_THRESHOLD_RANGE_LOW		0.396
#define CLOSE_THRESHOLD_RANGE_HIGH		0.405

#define VALID_TRIGGER_LINE_LOW			100
#define VALID_TRIGGER_LINE_HIGH			200

#define CREST_RATIO_IN_CREST_TROUGH		0.3
#define TROUGH_RATIO_IN_CREST_TROUGH	0.4

BOOL GetCrestTroughPoints(DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough);
BOOL IsOpenValue(DWORD dwTestOpenValue, DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough);
BOOL IsCloseValue(DWORD dwTestCloseValue, DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough);
BOOL RemoveTinyCrestTrough(DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough, DWORD* pdwMinGait, DWORD* pdwMaxGait, DWORD* pdwAverage);


BOOL APIENTRY DllMain( HANDLE hModule, 
                       DWORD  ul_reason_for_call, 
                       LPVOID lpReserved
					 )
{
    switch (ul_reason_for_call)
	{
		case DLL_PROCESS_ATTACH:
		case DLL_THREAD_ATTACH:
		case DLL_THREAD_DETACH:
		case DLL_PROCESS_DETACH:
			break;
    }
    return TRUE;
}


BOOL GetSmartWAParam(DWORD aGaitArray[], int nEleCount, DWORD* pdwStiOpen,  DWORD* pdwStiClose, DWORD aOutCrestTrough[], DWORD* pdwMinGait, DWORD* pdwMaxGait, DWORD* pdwAverage)
{
	if(aGaitArray == NULL || nEleCount < 10 || pdwStiOpen == NULL || pdwStiClose == NULL)
	{
		assert(FALSE);
		return FALSE;
	}

	DWORD* paCrestTrough = new DWORD[nEleCount];
	NULL_RETURN_FALSE(paCrestTrough);

	//  find the crest and trough points
	if(!GetCrestTroughPoints(aGaitArray, nEleCount, paCrestTrough))
	{
		SAFE_DELETE(paCrestTrough);
		return FALSE;
	}

	RemoveTinyCrestTrough(aGaitArray, nEleCount, paCrestTrough, pdwMinGait, pdwMaxGait, pdwAverage);


	int nCrestCount = 0;
	double dTotalCrest = 0.0;

	int nTroughCount = 0;
	double dTotalTrough = 0.0;

	for(int i = 0; i < nEleCount; i++)
	{
		if(paCrestTrough[i] == WAVE_CREST_FLAG)
		{
			nCrestCount++;
			dTotalCrest += aGaitArray[i];
		}
		else if(paCrestTrough[i] == WAVE_TROUGH_FLAG)
		{
			nTroughCount++;
			dTotalTrough += aGaitArray[i];
		}
	}

	double dAverageCrest = 0.0;
	if(nCrestCount > 0)
		dAverageCrest = dTotalCrest / nCrestCount;

	double dAverageTrough = 0.0;
	if(nTroughCount > 0)
		dAverageTrough = dTotalTrough / nTroughCount;

	double dRetCrest	= dAverageTrough + (dAverageCrest - dAverageTrough) * (1 - CREST_RATIO_IN_CREST_TROUGH);
	double dRetTrough	= dAverageTrough + (dAverageCrest - dAverageTrough) * TROUGH_RATIO_IN_CREST_TROUGH;

	*pdwStiOpen		= (DWORD)dRetCrest;
	*pdwStiClose	= (DWORD)dRetTrough;

	if(aOutCrestTrough)
	{
		memcpy(aOutCrestTrough, paCrestTrough, sizeof(DWORD) * nEleCount);
	}

	SAFE_DELETE(paCrestTrough);
	return TRUE;
}

BOOL GetCrestTroughPoints(DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough)
{
	NULL_RETURN_FALSE(paCrestTrough);
	memset(paCrestTrough, WAVE_COMMON_FLAG, sizeof(DWORD) * nEleCount);
	
	//  find the crest and trough points
	DWORD dwLineLen = nEleCount - 1;

	for(DWORD i = 0; i < 255; i++)
	{
		//  Start from 1, end at (n - 1)
		for(DWORD j = 1; j < dwLineLen; j++)
		{
			if(aGaitArray[j] == i)	// find a point, maybe a crest or trough
			{
				BOOL bMaybeCrest = FALSE;
				BOOL bMaybeTrough = FALSE;

				if(aGaitArray[j - 1] < i)
					bMaybeCrest = TRUE;

				if(aGaitArray[j - 1] > i)
					bMaybeTrough = TRUE;

				int k = 0;
				for(k = j + 1; k < nEleCount; k++)
				{
					if(aGaitArray[k] != i)
						break;
				}

				if(aGaitArray[k] < i && bMaybeCrest)
				{
					//  found a crest
					DWORD dwCrestPos = ((j - 1) + k) / 2;	//  maybe several points are the same as the crest
					paCrestTrough[dwCrestPos] = WAVE_CREST_FLAG;

					j = k;
					continue;
				}
					
				if(aGaitArray[k] > i && bMaybeTrough)
				{
					//  found a trough
					DWORD dwTroughPos = ((j - 1) + k) / 2;	//  maybe several points are the same as the trough
					paCrestTrough[dwTroughPos] = WAVE_TROUGH_FLAG;
					
					j = k;
					continue;
				}
			}
		}
	}
	
	return TRUE;
}


BOOL IsOpenValue(DWORD dwTestOpenValue, DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough)
{
	double dTotalSection = 0.0;
	int nSectionCount = 0;

	for(int i = 0; i < nEleCount; i++)
	{
		if(dwTestOpenValue == aGaitArray[i])
		{
			//  the test open-line is intersect the curve, determine the curve is upwards or adown.
			for(int j = i + 1; j < nEleCount; j++)
			{
				if(paCrestTrough[j] != WAVE_COMMON_FLAG)
				{
					//  the test open-line intersect a upwards curve  
					if(paCrestTrough[j] == WAVE_CREST_FLAG)
					{
						nSectionCount++;
						dTotalSection += (j - i);
					}
					
					//  from this point, to find another section 
					i = j;
					break;
				}
			}
		}
		else
		{
			if(i > 0 && dwTestOpenValue > aGaitArray[i - 1] && dwTestOpenValue < aGaitArray[i])
			{
				//  the test open-line is intersect the curve, determine the curve is upwards or adown.
				for(int j = i + 1; j < nEleCount; j++)
				{
					if(paCrestTrough[j] != WAVE_COMMON_FLAG)
					{
						//  the test open-line intersect a upwards curve  
						if(paCrestTrough[j] == WAVE_CREST_FLAG)
						{
							nSectionCount++;

							double dOffset = (dwTestOpenValue * 1.0 - aGaitArray[i - 1]) / (aGaitArray[i] - aGaitArray[i - 1]);
							double dExtra = 1 - dOffset;
							dTotalSection += (j - i + dExtra);
						}
						
						//  from this point, to find another section 
						i = j;
						break;
					}
				}
			}
		}

	}

	if(nSectionCount > 0)
	{
		double dAverage = dTotalSection / nSectionCount;
		double dAverageTime = dAverage * TIME_PER_POINT;

		if(dAverageTime > OPEN_THRESHOLD_RANGE_LOW && dAverageTime < OPEN_THRESHOLD_RANGE_HIGH)
			return TRUE;
	}

	return FALSE;
}


BOOL IsCloseValue(DWORD dwTestCloseValue, DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough)
{
	double dTotalSection = 0.0;
	int nSectionCount = 0;
	
	for(int i = 0; i < nEleCount; i++)
	{
		if(dwTestCloseValue == aGaitArray[i])
		{
			//  the test close-line is intersect the curve, determine the curve is upwards or adown.
			for(int j = i + 1; j < nEleCount; j++)
			{
				if(paCrestTrough[j] != WAVE_COMMON_FLAG)
				{
					//  the test close-line intersect a adown curve  
					if(paCrestTrough[j] == WAVE_TROUGH_FLAG)
					{
						nSectionCount++;
						dTotalSection += (j - i);
					}
					
					//  from this point, to find another section 
					i = j;
					break;
				}
			}
		}
		else
		{
			if(i > 0 && dwTestCloseValue < aGaitArray[i - 1] && dwTestCloseValue > aGaitArray[i])
			{
				//  the test close-line is intersect the curve, determine the curve is upwards or adown.
				for(int j = i + 1; j < nEleCount; j++)
				{
					if(paCrestTrough[j] != WAVE_COMMON_FLAG)
					{
						//  the test close-line intersect a adown curve  
						if(paCrestTrough[j] == WAVE_TROUGH_FLAG)
						{
							nSectionCount++;
							
							double dOffset = (aGaitArray[i - 1] - dwTestCloseValue * 1.0) / (aGaitArray[i - 1] - aGaitArray[i]);
							double dExtra = 1 - dOffset;
							dTotalSection += (j - i + dExtra);
						}
						
						//  from this point, to find another section 
						i = j;
						break;
					}
				}
			}
		}
		
	}
	
	if(nSectionCount > 0)
	{
		double dAverage = dTotalSection / nSectionCount;
		double dAverageTime = dAverage * TIME_PER_POINT;
		
		if(dAverageTime > CLOSE_THRESHOLD_RANGE_LOW && dAverageTime < CLOSE_THRESHOLD_RANGE_HIGH)
			return TRUE;
	}
	
	return FALSE;
}

#define COMMON_POINT_COUNT		4
#define TINY_WAVE_RANGE			16
#define GAIT_CHANGE_RANGE		30

BOOL RemoveTinyCrestTrough(DWORD aGaitArray[], int nEleCount, DWORD* paCrestTrough, DWORD* pdwMinGait, DWORD* pdwMaxGait, DWORD* pdwAverage)
{
 	DWORD dwMaxValue = 0;
	DWORD dwMinValue = 0xffff;
	for(int i = 0; i < nEleCount; i++)
	{
		if(paCrestTrough[i] == WAVE_CREST_FLAG)
		{
			if(aGaitArray[i] > dwMaxValue)
				dwMaxValue = aGaitArray[i];
		}

		if(paCrestTrough[i] == WAVE_TROUGH_FLAG)
		{
			if(aGaitArray[i] < dwMinValue)
				dwMinValue = aGaitArray[i];
		}
	}

	DWORD dwAverage = (dwMinValue + dwMaxValue) / 2;
	
	for(i = 0; i < nEleCount; i++)
	{
		if(paCrestTrough[i] == WAVE_CREST_FLAG)
		{
			if(aGaitArray[i] < dwAverage)
				paCrestTrough[i] = WAVE_COMMON_FLAG;
		}
		else if(paCrestTrough[i] == WAVE_TROUGH_FLAG)
		{
			if(aGaitArray[i] > dwAverage)
				paCrestTrough[i] = WAVE_COMMON_FLAG;
		}
	}

	
	*pdwMinGait = dwMinValue;
	*pdwMaxGait = dwMaxValue;
	*pdwAverage = dwAverage;

	DWORD dwMaxCrest = 0;
	DWORD dwMinTrough = 0xfff;

	int nPrevCrestPos = 0;
	int nPrevTroughPos = 0;
	for(i = 1; i < nEleCount - 1; i++)
	{
		if(paCrestTrough[i] == WAVE_CREST_FLAG)
		{
			paCrestTrough[i] = WAVE_COMMON_FLAG;

			if(aGaitArray[i] > dwMaxCrest)
			{
				nPrevCrestPos = i;
				dwMaxCrest = aGaitArray[i];
			}

			if(nPrevTroughPos != 0)
			{
				paCrestTrough[nPrevTroughPos] = WAVE_TROUGH_FLAG;

				nPrevTroughPos = 0;
				dwMinTrough = 0xfff;
			}
		}
		else if(paCrestTrough[i] == WAVE_TROUGH_FLAG)
		{
			paCrestTrough[i] = WAVE_COMMON_FLAG;
			
			if(aGaitArray[i] < dwMinTrough)
			{
				nPrevTroughPos = i;
				dwMinTrough = aGaitArray[i];
			}
			
			if(nPrevCrestPos != 0)
			{
				paCrestTrough[nPrevCrestPos] = WAVE_CREST_FLAG;
				
				nPrevCrestPos = 0;
				dwMaxCrest = 0;
			}
		}
	}

	return TRUE;
}